#include <SimpleFOC.h>
#include <Wire.h> // 需要添加I2C库
#include <WiFi.h>
#include <WebServer.h>
#include <ArduinoJson.h>

//配置WiFi信息
const char* ssid = "fourzkw";
const char* password = "zkw200512041315";// Web服务器和缓冲设置

WebServer server(80);
const unsigned long POST_INTERVAL = 30; // 最小处理间隔(ms)
const uint8_t BUFFER_SIZE = 20;        // 数据缓冲区大小

// 数据结构定义
struct JoyData {
  int x;
  int y;
  unsigned long timestamp;
};

// 环形缓冲区实现
class RingBuffer {
private:
  JoyData buffer[BUFFER_SIZE];
  uint8_t head = 0;
  uint8_t tail = 0;
  uint8_t count = 0;

public:
  bool push(const JoyData& data) {
    if (count >= BUFFER_SIZE) return false;
    buffer[head] = data;
    head = (head + 1) % BUFFER_SIZE;
    count++;
    return true;
  }

  bool pop(JoyData* data) {
    if (count == 0) return false;
    *data = buffer[tail];
    tail = (tail + 1) % BUFFER_SIZE;
    count--;
    return true;
  }

  bool isFull() const { return count >= BUFFER_SIZE; }
  bool isEmpty() const { return count == 0; }
};

RingBuffer dataBuffer;
unsigned long lastProcessTime = 0;


// 使用AS5600磁编码器 - I2C接口
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

// BLDC电机和驱动实例
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(4, 5, 6, 7);

void handleRoot() {
  String html = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
  <meta charset="UTF-8">
  <meta name="viewport" content="width=device-width, initial-scale=1.0">
  <style>
    body {
      font-family: Arial, sans-serif;
      max-width: 800px;
      margin: 0 auto;
      padding: 20px;
    }
    #speedControl {
      width: 100%;
      margin: 30px auto;
      text-align: center;
    }
    #speedSlider {
      width: 100%;
      margin: 20px 0;
    }
    #speedValue {
      font-size: 24px;
      margin: 10px 0;
    }
    .speedButtons {
      display: flex;
      justify-content: center;
      gap: 10px;
      margin: 20px 0;
    }
    .speedButtons button {
      width: 50px;
      height: 50px;
      font-size: 20px;
    }
    #pidControls {
      width: 100%;
      margin: 40px auto;
      padding: 15px;
      border-radius: 10px;
      box-shadow: 0 2px 15px rgba(0,0,0,0.1);
      background: rgba(240,240,240,0.95);
      border: 1px solid rgba(0,0,0,0.1);
    }
    #pidForm {
      display: flex;
      flex-direction: column;
      gap: 12px;
    }
    .input-group {
      display: flex;
      gap: 8px;
    }
    .input-group label {
      flex: 1;
      display: flex;
      flex-direction: column;
      font-size: 14px;
      color: #333;
    }
    .input-group input {
      padding: 8px;
      border: 1px solid #ccc;
      border-radius: 4px;
      font-size: 14px;
      width: 100%;
      box-sizing: border-box;
    }
    button {
      background: #4CAF50;
      color: white;
      padding: 10px;
      border: none;
      border-radius: 4px;
      cursor: pointer;
      font-size: 14px;
      transition: background 0.3s;
    }
    button:hover {
      background: #45a049;
    }
    h2, h3 {
      color: #444;
      text-align: center;
    }
  </style>
</head>
<body>
  <div id="speedControl">
    <h2>速度控制</h2>
    <div id="speedValue">0%</div>
    <input type="range" id="speedSlider" min="-100" max="100" value="0" step="1">
    <div class="speedButtons">
      <button id="decrease10">-10</button>
      <button id="decrease1">-1</button>
      <button id="reset">0</button>
      <button id="increase1">+1</button>
      <button id="increase10">+10</button>
    </div>
  </div>

  <div id="pidControls">
    <h3>PID参数设置</h3>
    <form id="pidForm">
      <div class="input-group">
        <label>
          P (比例)
          <input type="number" id="p" step="0.01" required>
        </label>
        <label>
          I (积分)
          <input type="number" id="i" step="0.001" required>
        </label>
        <label>
          D (微分)
          <input type="number" id="d" step="0.01" required>
        </label>
      </div>
      <button type="submit">更新参数</button>
    </form>
  </div>

  <script>
    const speedSlider = document.getElementById('speedSlider');
    const speedValue = document.getElementById('speedValue');
    const SEND_INTERVAL = 20;
    let currentSpeed = 0;
    let lastSentSpeed = null;
    const MAX_SPEED = 100; // 定义最大速度限制

    // 更新显示和发送数据，添加速度限制
    function updateSpeed(value) {
      // 限制速度范围在 -MAX_SPEED 到 MAX_SPEED 之间
      value = Math.max(-MAX_SPEED, Math.min(MAX_SPEED, parseInt(value)));
      currentSpeed = value;
      speedValue.textContent = currentSpeed + '%';
      speedSlider.value = currentSpeed;
    }

    // 发送数据到服务器
    function sendSpeed() {
      if (lastSentSpeed !== currentSpeed) {
        fetch('/control', {
          method: 'POST',
          headers: {'Content-Type': 'application/json'},
          body: JSON.stringify({x: currentSpeed, y: 0})
        }).catch(() => {});
        lastSentSpeed = currentSpeed;
      }
    }

    // 定时发送数据
    setInterval(sendSpeed, SEND_INTERVAL);

    // 滑块事件
    speedSlider.addEventListener('input', () => {
      updateSpeed(speedSlider.value);
    });

    // 按钮事件，添加速度限制
    document.getElementById('decrease10').addEventListener('click', () => {
      updateSpeed(currentSpeed - 10);
    });
    document.getElementById('decrease1').addEventListener('click', () => {
      updateSpeed(currentSpeed - 1);
    });
    document.getElementById('reset').addEventListener('click', () => {
      updateSpeed(0);
    });
    document.getElementById('increase1').addEventListener('click', () => {
      updateSpeed(currentSpeed + 1);
    });
    document.getElementById('increase10').addEventListener('click', () => {
      updateSpeed(currentSpeed + 10);
    });

    // PID表单处理
    document.getElementById('pidForm').addEventListener('submit', function(e) {
      e.preventDefault();
      
      const p = parseFloat(document.getElementById('p').value);
      const i = parseFloat(document.getElementById('i').value);
      const d = parseFloat(document.getElementById('d').value);
      
      if (isNaN(p) || isNaN(i) || isNaN(d)) {
        alert('请输入有效的数值');
        return;
      }

      fetch('/setpid', {
        method: 'POST',
        headers: {'Content-Type': 'application/json'},
        body: JSON.stringify({ p, i, d })
      })
      .then(response => {
        if (!response.ok) throw new Error('更新失败');
        return response.text();
      })
      .then(message => {
        alert('参数更新成功！');
        console.log('PID参数已更新:', {p, i, d});
      })
      .catch(error => {
        alert(error.message || '发生错误');
      });
    });
  </script>
</body>
</html>
)rawliteral";
  
  server.send(200, "text/html", html);
}

void handleControl() {
  static unsigned long lastPost = 0;
  const unsigned long now = millis();
  
  // 频率限制
  if (now - lastPost < POST_INTERVAL) {
    server.send(429, "text/plain", "Too Fast");
    return;
  }
  lastPost = now;

  // 数据解析
  if (!server.hasArg("plain")) {
    server.send(400, "text/plain", "Bad Request");
    return;
  }

  JoyData newData;
  JsonDocument doc;
  DeserializationError error = deserializeJson(doc, server.arg("plain"));
  
  if (error) {
    server.send(400, "text/plain", "Invalid JSON");
    return;
  }

  // 添加服务器端速度限制
  newData.x = constrain(doc["x"].as<int>(), -100, 100);
  newData.y = constrain(doc["y"].as<int>(), -100, 100);
  newData.timestamp = now;

  if (!dataBuffer.push(newData)) {
    server.send(507, "text/plain", "Buffer Full");
    return;
  }

  server.send(200, "text/plain", "OK");
}

void handleSetPID() {
  // 频率限制检查（可选）
  static unsigned long lastPIDUpdate = 0;
  if (millis() - lastPIDUpdate < 500) {
    server.send(429, "text/plain", "Too Frequent");
    return;
  }
  lastPIDUpdate = millis();

  // 解析JSON数据
  JsonDocument doc;
  DeserializationError error = deserializeJson(doc, server.arg("plain"));
  
  if (error) {
    server.send(400, "text/plain", "Invalid JSON");
    return;
  }

  // 获取参数值（带范围检查）
  float newP = doc["p"] | motor.PID_velocity.P; // 保持原值如果未提供
  float newI = doc["i"] | motor.PID_velocity.I;
  float newD = doc["d"] | motor.PID_velocity.D;

  // 参数有效性检查
  if (newP < 0 || newI < 0 || newD < 0) {
    server.send(400, "text/plain", "Invalid Parameters");
    return;
  }

  // 更新PID参数
  motor.PID_velocity.P = newP;
  motor.PID_velocity.I = newI;
  motor.PID_velocity.D = newD;

  // 可选：持久化到EEPROM
  // savePIDToEEPROM(newP, newI, newD);

  // 调试输出
  Serial.printf("PID Updated: P=%.2f I=%.3f D=%.2f\n", newP, newI, newD);
  
  server.send(200, "text/plain", "PID Updated");
}

Commander command = Commander(Serial);
void onMotor(char* cmd){ command.motor(&motor, cmd); }

void setup() {
  // 串口监视
  Serial.begin(115200);

  // 初始化WiFi连接
  WiFi.persistent(false);
  WiFi.mode(WIFI_STA);
  WiFi.setSleep(false);
  WiFi.begin(ssid, password);

  Serial.print("Connecting");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  
  Serial.printf("\nConnected! IP: %s\n", WiFi.localIP().toString().c_str());
  
  server.on("/", handleRoot);
  server.on("/control", handleControl);
  server.on("/setpid",HTTP_POST, handleSetPID);
  server.begin();
  
  // 初始化I2C通信
  Wire.setPins(17, 18);
  Wire.begin(); 

  // 启用调试输出
   SimpleFOCDebug::enable(&Serial);

  // 初始化磁编码器硬件
  sensor.init(); // AS5600自动初始化，无需额外配置
  
  // 将电机链接到传感器
  motor.linkSensor(&sensor);

  // 驱动器配置
  driver.voltage_power_supply = 12; // 电源电压[V]
  driver.init();
  motor.linkDriver(&driver);

  // 设置控制模式为扭矩控制
  motor.torque_controller = TorqueControlType::voltage; // 电压模式扭矩控制
  motor.controller = MotionControlType::torque;

  // 控制器参数配置
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 0.2;
  motor.PID_velocity.D = 0;
  motor.voltage_limit = 6;    // 电压限制
  motor.LPF_velocity.Tf = 0.01; // 速度低通滤波

  // 角度环参数（如果使用角度控制）
  // motor.P_angle.P = 20;
  motor.velocity_limit = 100;   // 最大速度限制

   // 启用串口监视（可选）
   motor.useMonitoring(Serial);

  // 初始化电机
  motor.init();

  // 初始化FOC算法
  motor.initFOC();

  // 设置初始目标值
  motor.target = 10; // 20V电压目标

  Serial.println(F("电机已初始化，使用AS5600编码器"));
  _delay(1000);
}

int target = 10;

void loop() {
  server.handleClient();

  // 运行FOC算法
  motor.loopFOC();

  JoyData data;
  // 检查是否成功从缓冲区获取数据
  if (dataBuffer.pop(&data)) {
    // 限制速度范围在-100到100之间
    int speed = constrain(data.x, -100, 100);
    // 执行运动控制
    target = speed; // 设置目标速度
    motor.move(target);
  }
  // 如果没有新数据，电机将保持当前速度
}